#ifndef STX_ALGORITHM_SORT_HPP
#define STX_ALGORITHM_SORT_HPP

#include <stx/algorithm/common.hpp>
#include <stx/algorithm/swap.hpp>
#include <stx/algorithm/min_element.hpp>
#include <stx/core/scoped_ptr.hpp>

namespace stx {

// stable, 2-3 compares, 0-2 swaps

template <class Compare, class ForwardIterator>
inline unsigned
sort3_impl(ForwardIterator x, ForwardIterator y, ForwardIterator z, Compare c)
{
    unsigned r = 0;
    if (!c(*y, *x)) {        // if x <= y
        if (!c(*z, *y)) {    // if y <= z
            return r;        // x <= y && y <= z
        }                    // x <= y && y > z
        swap(*y, *z);        // x <= z && y < z
        r = 1;
        if (c(*y, *x)) {     // if x > y
            swap(*x, *y);    // x < y && y <= z
            r = 2;
        }
        return r;            // x <= y && y < z
    }
    if (c(*z, *y)) {         // x > y, if y > z
        swap(*x, *z);        // x < y && y < z
        r = 1;
        return r;
    }
    swap(*x, *y);            // x > y && y <= z
    r = 1;                       // x < y && x <= z
    if (c(*z, *y)) {         // if y > z
        swap(*y, *z);        // x <= y && y < z
        r = 2;
    }
    return r;
}                            // x <= y && y <= z

// stable, 3-6 compares, 0-5 swaps

template <class Compare, class ForwardIterator>
inline unsigned
sort4_impl(
    ForwardIterator x1,
    ForwardIterator x2,
    ForwardIterator x3,
    ForwardIterator x4,
    Compare c)
{
    unsigned r = sort3_impl<Compare>(x1, x2, x3, c);
    if (c(*x4, *x3)) {
        swap(*x3, *x4);
        ++r;
        if (c(*x3, *x2)) {
            swap(*x2, *x3);
            ++r;
            if (c(*x2, *x1)) {
                swap(*x1, *x2);
                ++r;
            }
        }
    }
    return r;
}

// stable, 4-10 compares, 0-9 swaps

template <class Compare, class ForwardIterator>
inline unsigned
sort5_impl(
    ForwardIterator x1,
    ForwardIterator x2,
    ForwardIterator x3,
    ForwardIterator x4,
    ForwardIterator x5,
    Compare c)
{
    unsigned r = sort4_impl<Compare>(x1, x2, x3, x4, c);
    if (c(*x5, *x4)) {
        swap(*x4, *x5);
        ++r;
        if (c(*x4, *x3)) {
            swap(*x3, *x4);
            ++r;
            if (c(*x3, *x2)) {
                swap(*x2, *x3);
                ++r;
                if (c(*x2, *x1)) {
                    swap(*x1, *x2);
                    ++r;
                }
            }
        }
    }
    return r;
}

// Assumes size > 0
template <class Compare, class BirdirectionalIterator>
inline void
selection_sort_impl(BirdirectionalIterator first, BirdirectionalIterator last, Compare comp)
{
    BirdirectionalIterator lm1 = last;
    for (--lm1; first != lm1; ++first) {
        BirdirectionalIterator i =
            stx::min_element<BirdirectionalIterator, typename add_lvalue_reference<Compare>::type>(first, last, comp);
        if (i != first) {
            swap(*first, *i);
        }
    }
}

template <class Compare, class BirdirectionalIterator>
inline void
insertion_sort_impl(BirdirectionalIterator first, BirdirectionalIterator last, Compare comp)
{
    typedef typename std::iterator_traits<BirdirectionalIterator>::value_type value_type;
    if (first != last) {
        BirdirectionalIterator i = first;
        for (++i; i != last; ++i) {
            BirdirectionalIterator j = i;
            value_type t(stx::move(*j));
            for (BirdirectionalIterator k = i; k != first && comp(t,  *--k); --j) {
                *j = stx::move(*k);
            }
            *j = stx::move(t);
        }
    }
}

template <class Compare, class RandomAccessIterator>
inline void
insertion_sort_impl_3(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
{
    typedef typename std::iterator_traits<RandomAccessIterator>::value_type value_type;
    RandomAccessIterator j = first + 2;
    sort3_impl<Compare>(first, first + 1, j, comp);
    for (RandomAccessIterator i = j + 1; i != last; ++i) {
        if (comp(*i, *j)) {
            value_type t(stx::move(*i));
            RandomAccessIterator k = j;
            j = i;
            do {
                *j = stx::move(*k);
                j = k;
            } while (j != first && comp(t, *--k));
            *j = stx::move(t);
        }
        j = i;
    }
}

template <class Compare, class RandomAccessIterator>
inline bool
insertion_sort_incomplete_impl(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
{
    switch (last - first) {
    case 0:
    case 1:
        return true;
    case 2:
        if (comp(*--last, *first)) {
            swap(*first, *last);
        }
        return true;
    case 3:
        stx::sort3_impl<Compare>(first, first+1, --last, comp);
        return true;
    case 4:
        stx::sort4_impl<Compare>(first, first+1, first+2, --last, comp);
        return true;
    case 5:
        stx::sort5_impl<Compare>(first, first+1, first+2, first+3, --last, comp);
        return true;
    }
    typedef typename std::iterator_traits<RandomAccessIterator>::value_type value_type;
    RandomAccessIterator j = first + 2;
    sort3_impl<Compare>(first, first + 1, j, comp);
    const unsigned limit = 8;
    unsigned count = 0;
    for (RandomAccessIterator i = j + 1; i != last; ++i) {
        if (comp(*i, *j)) {
            value_type t(stx::move(*i));
            RandomAccessIterator k = j;
            j = i;
            do {
                *j = stx::move(*k);
                j = k;
            } while (j != first && comp(t, *--k));
            *j = stx::move(t);
            if (++count == limit) {
                return ++i == last;
            }
        }
        j = i;
    }
    return true;
}

template <class Compare, class BirdirectionalIterator>
inline void
insertion_sort_move_impl(
    BirdirectionalIterator first1,
    BirdirectionalIterator last1,
    typename std::iterator_traits<BirdirectionalIterator>::value_type* first2,
    Compare comp)
{
    typedef typename std::iterator_traits<BirdirectionalIterator>::value_type value_type;
    if (first1 != last1) {
        destruct_n d(0);
        scoped_ptr<value_type, destruct_n&> h(first2, d);
        value_type* last2 = first2;
        ::new(last2) value_type(stx::move(*first1));
        d.incr((value_type*)0);
        for (++last2; ++first1 != last1; ++last2) {
            value_type* j2 = last2;
            value_type* i2 = j2;
            if (comp(*first1, *--i2)) {
                ::new(j2) value_type(stx::move(*i2));
                d.incr((value_type*)0);
                for (--j2; i2 != first2 && comp(*first1,  *--i2); --j2)
                    *j2 = stx::move(*i2);
                *j2 = stx::move(*first1);
            } else {
                ::new(j2) value_type(stx::move(*first1));
                d.incr((value_type*)0);
            }
        }
        h.release();
    }
}

template <class Compare, class RandomAccessIterator>
inline void
sort_impl(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
{
    // Compare is known to be a reference type
    typedef typename std::iterator_traits<RandomAccessIterator>::difference_type difference_type;
    typedef typename std::iterator_traits<RandomAccessIterator>::value_type value_type;
    const difference_type limit = 6;
//    const difference_type limit =
//        is_trivially_copy_constructible<value_type>::value &&
//        is_trivially_copy_assignable<value_type>::value ? 30 : 6;
    while (true) {
    restart_:
        difference_type len = last - first;
        switch (len) {
        case 0:
        case 1:
            return;
        case 2:
            if (comp(*--last, *first)) {
                swap(*first, *last);
            }
            return;
        case 3:
            stx::sort3_impl<Compare>(first, first+1, --last, comp);
            return;
        case 4:
            stx::sort4_impl<Compare>(first, first+1, first+2, --last, comp);
            return;
        case 5:
            stx::sort5_impl<Compare>(first, first+1, first+2, first+3, --last, comp);
            return;
        }
        if (len <= limit) {
            stx::insertion_sort_impl_3<Compare>(first, last, comp);
            return;
        }
        // len > 5
        RandomAccessIterator m = first;
        RandomAccessIterator lm1 = last;
        --lm1;
        unsigned n_swaps;
        {
            difference_type delta;
            if (len >= 1000) {
                delta = len/2;
                m += delta;
                delta /= 2;
                n_swaps = stx::sort5_impl<Compare>(first, first + delta, m, m+delta, lm1, comp);
            } else {
                delta = len/2;
                m += delta;
                n_swaps = stx::sort3_impl<Compare>(first, m, lm1, comp);
            }
        }
        // *m is median
        // partition [first, m) < *m and *m <= [m, last)
        // (this inhibits tossing elements equivalent to m around unnecessarily)
        RandomAccessIterator i = first;
        RandomAccessIterator j = lm1;
        // j points beyond range to be tested, *m is known to be <= *lm1
        // The search going up is known to be guarded but the search coming down isn't.
        // Prime the downward search with a guard.
        if (!comp(*i, *m)) { // if *first == *m
            // *first == *m, *first doesn't go in first part
            // manually guard downward moving j against i
            while (1) {
                if (i == --j) {
                    // *first == *m, *m <= all other elements
                    // Parition instead into [first, i) == *first and *first < [i, last)
                    ++i;  // first + 1
                    j = last;
                    if (!comp(*first, *--j)) { // we need a guard if *first == *(last-1)
                        while (1) {
                            if (i == j) {
                                return;  // [first, last) all equivalent elements
                            }
                            if (comp(*first, *i)) {
                                swap(*i, *j);
                                ++n_swaps;
                                ++i;
                                break;
                            }
                            ++i;
                        }
                    }
                    // [first, i) == *first and *first < [j, last) and j == last - 1
                    if (i == j) {
                        return;
                    }
                    while (1) {
                        while (!comp(*first, *i)) {
                            ++i;
                        }
                        while (comp(*first, *--j)) {
                        }
                        if (i >= j) {
                            break;
                        }
                        swap(*i, *j);
                        ++n_swaps;
                        ++i;
                    }
                    // [first, i) == *first and *first < [i, last)
                    // The first part is sorted, sort the secod part
                    // stx::sort_impl<Compare>(i, last, comp);
                    first = i;
                    goto restart_;
                }
                if (comp(*j, *m)) {
                    swap(*i, *j);
                    ++n_swaps;
                    break;  // found guard for downward moving j, now use unguarded partition
                }
            }
        }
        // It is known that *i < *m
        ++i;
        // j points beyond range to be tested, *m is known to be <= *lm1
        // if not yet partitioned...
        if (i < j) {
            // known that *(i - 1) < *m
            // known that i <= m
            while (1) {
                // m still guards upward moving i
                while (comp(*i, *m)) {
                    ++i;
                }
                // It is now known that a guard exists for downward moving j
                while (!comp(*--j, *m)) {
                }
                if (i > j) {
                    break;
                }
                swap(*i, *j);
                ++n_swaps;
                // It is known that m != j
                // If m just moved, follow it
                if (m == i) {
                    m = j;
                }
                ++i;
            }
        }
        // [first, i) < *m and *m <= [i, last)
        if (i != m && comp(*m, *i)) {
            swap(*i, *m);
            ++n_swaps;
        }
        // [first, i) < *i and *i <= [i+1, last)
        // If we were given a perfect partition, see if insertion sort is quick...
        if (n_swaps == 0) {
            bool fs = stx::insertion_sort_incomplete_impl<Compare>(first, i, comp);
            if (stx::insertion_sort_incomplete_impl<Compare>(i+1, last, comp)) {
                if (fs) {
                    return;
                }
                last = i;
                continue;
            } else {
                if (fs) {
                    first = ++i;
                    continue;
                }
            }
        }
        // sort smaller range with recursive call and larger with tail recursion elimination
        if (i - first < last - i) {
            stx::sort_impl<Compare>(first, i, comp);
            // stx::sort_impl<Compare>(i+1, last, comp);
            first = ++i;
        } else {
            stx::sort_impl<Compare>(i+1, last, comp);
            // stx::sort_impl<Compare>(first, i, comp);
            last = i;
        }
    }
}

// This forwarder keeps the top call and the recursive calls using the same instantiation, forcing a reference Compare
template <class RandomAccessIterator, class Compare>
inline void
sort(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
{
    typedef typename stx::add_lvalue_reference<Compare>::type comp_ref;
    stx::sort_impl<comp_ref>(first, last, comp);
}

template <class RandomAccessIterator>
inline void
sort(RandomAccessIterator first, RandomAccessIterator last)
{
    stx::sort(first, last, stx::less<typename std::iterator_traits<RandomAccessIterator>::value_type>());
}

} // namespace stx

#endif // STX_ALGORITHM_SORT_HPP
